
#include "stm32f10x_flash.h"
#include "sysdef.h"
#include "flash.h"
#include <stddef.h>
#include <stdlib.h>
#include <string.h>

#define FLASH_SIZE_KB_ONCHIP	512
#define FLASH_PAGE_SIZE_ONCHIP	2048

extern char __flash_user_start[];
extern char __flash_user_end[];

extern int flash_read_onchip(uint32_t address, uint8_t *data, uint32_t len);

static uint16_t flash_read_halfword_onchip(uint32_t address) {
	return *(volatile uint16_t*)address;
}

static int flash_write_no_check_onchip(uint32_t address, uint16_t *data, uint32_t len) { 			 		 
	uint16_t i;
	for(i=0; i<len/2; i++) {
		if(FLASH_ProgramHalfWord(address, data[i]) != FLASH_COMPLETE) return -1;
	    address += 2;
	}

	return 0;
}

int flash_write_onchip(uint32_t address, uint8_t *data, uint32_t len) {
	uint32_t start_addr = address - FLASH_BASE;
	uint32_t end_addr = address - FLASH_BASE + len;
	uint32_t start_page_pos = start_addr / FLASH_PAGE_SIZE_ONCHIP;
	uint32_t end_page_pos = end_addr / FLASH_PAGE_SIZE_ONCHIP;
	uint32_t start_wbyte_pos_inpage = 0;
	uint32_t end_wbyte_pos_inpage = 0;
	uint8_t *flash_rw_buf;

	if(address < (uint32_t)__flash_user_start || (address >= (uint32_t)__flash_user_end)) return -1;
	FLASH_Unlock();

	flash_rw_buf = malloc(FLASH_PAGE_SIZE_ONCHIP);
	if(!flash_rw_buf) {
		FLASH_Lock();
		return -1;
	}

	for(uint32_t pos = start_page_pos; pos <= end_page_pos; pos++) {
		int i;

		if(pos == start_page_pos) start_wbyte_pos_inpage = start_addr % FLASH_PAGE_SIZE_ONCHIP;
		else start_wbyte_pos_inpage = 0;

		if(pos == end_page_pos) end_wbyte_pos_inpage = end_addr % FLASH_PAGE_SIZE_ONCHIP;
		else end_wbyte_pos_inpage = FLASH_PAGE_SIZE_ONCHIP;

		flash_read_onchip(pos * FLASH_PAGE_SIZE_ONCHIP + FLASH_BASE, flash_rw_buf, FLASH_PAGE_SIZE_ONCHIP);

		if(memcmp(flash_rw_buf + start_wbyte_pos_inpage, data, end_wbyte_pos_inpage - start_wbyte_pos_inpage) == 0)
			goto next_loop;

		for(i=0; i<FLASH_PAGE_SIZE_ONCHIP; i++) {
			if(flash_rw_buf[i] != 0xFF) break;
		}

		if(i < FLASH_PAGE_SIZE_ONCHIP) {
			if(FLASH_ErasePage(pos * FLASH_PAGE_SIZE_ONCHIP + FLASH_BASE) != FLASH_COMPLETE) {
				FLASH_Lock();
				free(flash_rw_buf);
				return -1;
			}
		}

		memcpy(flash_rw_buf + start_wbyte_pos_inpage, data, end_wbyte_pos_inpage - start_wbyte_pos_inpage);
		if(flash_write_no_check_onchip(pos * FLASH_PAGE_SIZE_ONCHIP + FLASH_BASE, (uint16_t *)flash_rw_buf, FLASH_PAGE_SIZE_ONCHIP)) {
			FLASH_Lock();
			free(flash_rw_buf);
			return -1;
		}

	next_loop:
		data += end_wbyte_pos_inpage - start_wbyte_pos_inpage;
	}

	FLASH_Lock();
	free(flash_rw_buf);
	return 0;
}

int flash_read_onchip(uint32_t address, uint8_t *data, uint32_t len) {
	uint32_t end_addr = address + len;
	uint16_t read_halfword;
	uint32_t read_len = 0;

	if(address < (uint32_t)__flash_user_start || (address >= (uint32_t)__flash_user_end)) return -1;

	if(!len) return 0;

	if(address % 2) {
		read_halfword = flash_read_halfword_onchip(address - 1);
		data[read_len++] = (uint8_t)(read_halfword >> 8);
		address++;
	}

	while(end_addr - address >= 2) {
		read_halfword = flash_read_halfword_onchip(address);
		data[read_len++] = (uint8_t)read_halfword;
		data[read_len++] = (uint8_t)(read_halfword >> 8);
		address += 2;
	}

	if(end_addr != address) {
		read_halfword = flash_read_halfword_onchip(address);
		data[read_len++] = (uint8_t)read_halfword;
		address++;
	}

	return 0;
}

int flash_erase_onchip(uint32_t address, uint32_t len) {
	uint32_t start_addr = address - FLASH_BASE;
	uint32_t end_addr = address - FLASH_BASE + len;
	uint32_t start_page_pos = start_addr / FLASH_PAGE_SIZE_ONCHIP;
	uint32_t end_page_pos = end_addr / FLASH_PAGE_SIZE_ONCHIP;
	uint8_t *flash_rw_buf;

	if(address < (uint32_t)__flash_user_start || (address >= (uint32_t)__flash_user_end)) return -1;
	FLASH_Unlock();

	flash_rw_buf = malloc(FLASH_PAGE_SIZE_ONCHIP);
	if(!flash_rw_buf) {
		FLASH_Lock();
		return -1;
	}

	for(uint32_t pos = start_page_pos; pos <= end_page_pos; pos++) {
		int i;

		flash_read_onchip(pos * FLASH_PAGE_SIZE_ONCHIP + FLASH_BASE, flash_rw_buf, FLASH_PAGE_SIZE_ONCHIP);
		for(i=0; i<FLASH_PAGE_SIZE_ONCHIP; i++) {
			if(flash_rw_buf[i] != 0xFF) break;
		}

		if(i < FLASH_PAGE_SIZE_ONCHIP) {
			if(FLASH_ErasePage(pos * FLASH_PAGE_SIZE_ONCHIP + FLASH_BASE) != FLASH_COMPLETE) {
				FLASH_Lock();
				free(flash_rw_buf);
				return -1;
			}
		}
	}

	FLASH_Lock();
	free(flash_rw_buf);
	return 0;
}

int flash_open_onchip(void) {
    flash_t *flash = flash_find_by_type(FLASH_TYPE_ONCHIP);
    flash->info.start_addr = FLASH_BASE;
    flash->info.size = __flash_user_end - __flash_user_start;
    flash->info.block_size = FLASH_PAGE_SIZE_ONCHIP;
    return 0;
}